#include <WProgram.h>
#include <stddef.h>
#include <stdlib.h>
#include <stdint.h>
#include "i2cmaster.h"
//#include "cppfix.h"
#include "maxim.h"
#include "utils.h"
#include "PrintBuf.h"
#include "registers.h"

/*
 * ================== Declare Statics here
 */
uint16_t RegUVthrSet::ThrValue;
uint16_t RegUVthrClear::ThrValue;
uint16_t RegOVthrSet::ThrValue;
uint16_t RegOVthrClear::ThrValue;


/*
 * ================== Register
 */

Register::Register(uint8_t nd) {
  setNumDevices(nd);
  init(0);
}

void Register::setNumDevices(uint8_t nd)
{
  //assert(nd<MAXDEVICES);
  numDevices=nd;
}

void Register::init(uint8_t addr) {
  address = addr;
  for (uint8_t i=0;i<numDevices;i++) {
    data[i]=0;
  }
  dP = (uint8_t*)data;
}
  
void Register::addData(uint8_t d) {
  //assert(dP < (uint8_t*)(data + numDevices));
  *dP++ = d;
}

boolean Register::isAlarm(uint8_t dataCheck) {
  return ((dataCheck & 0x80) == 1);
}

boolean Register::isPecError(uint8_t dataCheck) {
  return ((dataCheck & 1) == 1);
}

uint16_t Register::getValue(uint8_t deviceNumber) {
  //assert(deviceNumber >=1 && deviceNumber <= numDevices);

  deviceNumber--;               // convert to 1-based

  // ensure we've actually set this device data.
  //assert(dP > (uint8_t*)(data+deviceNumber));
  return data[deviceNumber];
}

boolean Register::readFromDevice()
{
  Pec pec;
  uint8_t cmd;
  int ret;
  uint8_t dataCheck;

  if (address == 0) return false;

  cmd = BROADCAST_ADDRESS + I2C_WRITE;
  i2c_start_wait(cmd);
  pec.add(cmd);

  cmd = address;
  if (i2c_write(cmd)) {
    PrintBuf::singleton()->add("Write Register!: ");
    PrintBuf::singleton()->add(cmd);
    return false;
  }
  pec.add(cmd);

  cmd = BROADCAST_ADDRESS + I2C_READ;
  i2c_rep_start(cmd);
  pec.add(cmd);

  // Should be able to read 2 bytes/device
  for (uint8_t dn=1;dn<=numDevices;dn++) {

    // read 1st byte
    ret = i2c_readAck();
    pec.add(ret);
    addData(ret);

    // read 2nd byte
    ret = i2c_readAck();
    pec.add(ret);
    addData(ret);
  }

  //Serial.println(debugLine++);

  // should recv dataCheck & PEC.
  dataCheck = i2c_readAck();
  pec.add(dataCheck);
  if (isAlarm(dataCheck)) {
    PrintBuf::singleton()->add("Alarm,dataCheck!: ",PrintBuf::EQUAL); 
    PrintBuf::singleton()->add(getAddress());
  }

  //Serial.println(debugLine++);

  // now PEC
  ret = i2c_readAck();
  if (!pec.verify(ret)) {
    PrintBuf::singleton()->add("PEC!: ",PrintBuf::EQUAL); 
    PrintBuf::singleton()->add(getAddress());

    (void)i2c_readNak();
    i2c_stop();
    return false;
  }

  (void)i2c_readNak();
  i2c_stop();

  return true;
}

void Register::print()
{
  // dP is the cursor to the end of data.
  uint16_t* p = data;
  Serial.print("Reg Addr: 0x");Serial.println(address,HEX);

  for (uint8_t dn=1;dn<=numDevices && (uint8_t*)p<dP ;dn++) {
    Serial.print("Reading device: 0x"); Serial.println(dn,HEX);
    Serial.println(*p, HEX);
  }
}

void Register::printError(uint8_t deviceNum, char *err)
{
  PrintBuf::singleton()->add("[device register]",PrintBuf::EQUAL); 
  PrintBuf::singleton()->add(deviceNum,PrintBuf::SPACE);
  PrintBuf::singleton()->add(address, PrintBuf::SPACE); 
  PrintBuf::singleton()->add("When",PrintBuf::SPACE);
  PrintBuf::singleton()->add(err);
}

/** Device address encoding:
    D7 1 Write ignored; read back 1.
    D6 0 Write ignored; read back 0.
    D5 A0 I2C device address. A0 is the LSB. 
    D4 A1
    D3 A2
    D2 A3
    D1 A4
    D0 0 Write ignored; read back 0.
**/
uint8_t Register::deviceNum2Address(uint8_t num)
{
  return 
    (0x80 |                     // set D7 & D6
     ((num&0x01) << 5) |        // D5 with A0
     ((num&0x02) << 4) |        // D4 with A1
     ((num&0x04) << 3) |        // D3 with A2
     ((num&0x08) << 2) |        // D2 with A3
     ((num&0x10) << 1));        // D1 with A4
}

boolean Register::writeAllDevices(uint16_t value)
{
  return writeDevice(value,0);
}

boolean Register::writeDevice(uint16_t value, uint8_t deviceNum)
{
  uint8_t deviceAddress;
  if (deviceNum == 0) {
    deviceAddress = BROADCAST_ADDRESS;
  } else if (deviceNum > MAXDEVICES) {
    return false;
  } else {
    deviceAddress = deviceNum2Address(deviceNum);
  }

  Pec pec;
  uint8_t cmd;

  cmd = deviceAddress + I2C_WRITE;
  i2c_start_wait(cmd);
  pec.add(cmd);

  cmd = address;
  if (i2c_write(cmd)) {
    i2c_stop();

    printError(deviceNum, "writing address"); 
    return false;
  }
  pec.add(cmd);
 
  // write LSB first.
  cmd = value & 0xFF;
  if (i2c_write(cmd)) {
    i2c_stop();

    printError(deviceNum, "writing LSB"); 
    return false;
  }
  pec.add(cmd);

  cmd = value >> 8;
  if (i2c_write(cmd)) {
    i2c_stop();

    printError(deviceNum, "writing MSB"); 
    return false;
  }
  pec.add(cmd);
  
  if (i2c_write(pec.getCRC())) {
    i2c_stop();

    printError(deviceNum, "writing PEC.");
    return false;
  }

  i2c_stop();
  return true;
}

/*
 * ================== RegStatus
 */
RegStatus::RegStatus(uint8_t numDevices)
  : Register(numDevices)
{
  init(0x02);                   // status register address
}

boolean RegStatus::clearStatus()
{
  return writeAllDevices(0);
}

boolean RegStatus::clearStatus(uint8_t deviceNum)
{
  return writeDevice(0,deviceNum);
}

// D15 is RSTAT bit. 1000 0H 0H 0H = 8000
boolean RegStatus::isReset(uint8_t deviceNum)
{
  uint16_t value = getValue(deviceNum);
  return ((value & 0x8000) != 0);
}
// D8 is ALRTACK bit 0H 0001 0H 0H = 100
boolean RegStatus::isAlrtAck(uint8_t deviceNum)
{
  uint16_t value = getValue(deviceNum);
  return ((value & 0x100) != 0);
}

uint8_t RegStatus::isAlrtAck()
{
  for (uint8_t dn=1; dn<=getNumDevices();dn++) {
    if (isAlrtAck(dn)) {
      return dn;
    }
  }
  return 0;
}

// D13 is UnderVoltage bit 0010 0H 0H 0H = 2000
boolean RegStatus::isUnderVoltage(uint8_t deviceNum)
{
  uint16_t value = getValue(deviceNum);
  return ((value & 0x2000) != 0);
}

boolean RegStatus::isUnderVoltage()
{
  for (uint8_t dn=1; dn<=getNumDevices();dn++) {
    if (isUnderVoltage(dn)) {
      return true;
    }
  }
  return false;
}

boolean RegStatus::read()
{
  return readFromDevice();
}

/*
 * ================== RegCellen
 */

RegCellen::RegCellen(uint8_t nd)
  : Register(nd)
{
  init(0x09);
}

// Set all bits D11:D0 to enable all cells
boolean RegCellen::enableAllCells() {
  return writeAllDevices(AllCellsEnabled);
}

boolean RegCellen::enableCells(uint16_t cellNumberMask)
{
  return writeAllDevices(cellNumberMask);
}

boolean RegCellen::enableCells(uint16_t cellNumberMask, 
                               uint8_t deviceNum)
{
  return writeDevice(cellNumberMask, deviceNum);
}

void RegCellen::setEnableCellMask(uint16_t& cellMask, uint8_t cellNumber) 
{
  //assert(cellNumber>=1 && cellNumber<=12);
    cellMask |= ((uint16_t)1 << (cellNumber-1));
}

void RegCellen::setDisableCellMask(uint16_t& cellMask, 
                                   uint8_t cellNumber)
{
  //assert(cellNumber>=1 && cellNumber<=12);
  cellMask &= ~((uint16_t)1 << (cellNumber-1));
}

void RegCellen::setEnableAllCellMask(uint16_t& cellMask)
{
  cellMask = AllCellsEnabled;
}

/*
 * ================== RegScanctrl
 */

RegScanctrl::RegScanctrl(uint8_t nd)
  : Register(nd)
{
  init(0x0D);
}

// Low bit, D0 = 1
boolean RegScanctrl::enable()
{
  return writeAllDevices(1);
}

// D0 = 0
boolean RegScanctrl::disable()
{
  return writeAllDevices(0);
}


/*
 * ================== RegCellData
 */

RegCellData::RegCellData(uint8_t nd) : Register(nd){
}

void RegCellData::initCellNumber(uint8_t cn) 
{
  Register::init(0x1F+cn);
  cellNumber = cn;
}

uint16_t RegCellData::getVoltageValue(uint8_t deviceNumber) {
  uint16_t cellData = getValue(deviceNumber);
  // Serial.println(cellData,HEX);
  uint16_t val = (cellData >> 4);
  //Serial.println(val,HEX);
  return val;
}       

float RegCellData::getVoltage(uint8_t deviceNumber) {
  uint16_t val = getVoltageValue(deviceNumber);
  return (val * ADC_VOLTAGE_RESOLUTION);
}

void RegCellData::print()
{
  Serial.print("Cell #: 0x"); Serial.println(cellNumber,HEX);
  Register::print();
}

/*
 * ================== RegDeviceAddress
 */

RegDeviceAddress::RegDeviceAddress(uint8_t nd)
  : Register(nd)
{
  init(0x01);                   // address Register
}


/* MSB of Address Register for Writing Last Address
   D15 0
   D14 0 Write ignored; read back 0.
   D13 0
   D12 LA4  The last address bits are used to support the 
            SMBus ladder alarm feature and the errorchecking 
            bytes of the READALL command. These bits
            are set by the SETLASTADDRESS command and 
            correspond to the A[4:0] device address bits of the 
            last device in the chain.  Once properly
            set in all nodes, the alarm heartbeat function begins.
   D11 LA3
   D10 LA2
   D9 LA1
   D8 LA0
*/
boolean RegDeviceAddress::setLastAddress() 
{
  // Works out that the MSB is equivalent to the number of devices
  // LSB is 0.
  uint16_t value = (uint16_t)getNumDevices() << 8;
  return writeAllDevices(value);
}

/*
 * ================== RegUVthrSet
 *
 * Address = 1A
 * Bits D15:D4 set the 12 bits of the threshold.
 *
 */
RegUVthrSet::RegUVthrSet(uint8_t nd) : Register(nd)
{
  init(0x1A);
}

// shift the threshold value by 4 bits
boolean RegUVthrSet::writeAllDevices()
{
  uint16_t value = (ThrValue << 4);
  return Register::writeAllDevices(value);
}

void RegUVthrSet::setThreshold(uint16_t value)
{
  ThrValue = value;
}

void RegUVthrSet::setThreshold(float value)
{
  ThrValue = (value / ADC_VOLTAGE_RESOLUTION);
}

/*
 * ================== RegUVthrClear
 *
 * Address = 1B
 * Bits D15:D4 set the 12 bits of the threshold.
 *
 */
RegUVthrClear::RegUVthrClear(uint8_t nd) : Register(nd)
{
  init(0x1B);
}

// shift the threshold value by 4 bits
boolean RegUVthrClear::writeAllDevices()
{
  uint16_t value = (ThrValue << 4);
  return Register::writeAllDevices(value);
}

void RegUVthrClear::setThreshold(uint16_t value)
{
  ThrValue = value;
}

void RegUVthrClear::setThreshold(float value)
{
  ThrValue = (value / ADC_VOLTAGE_RESOLUTION);
}

/* ================== RegOVthrSet
 *
 * Address = 19
 * Bits D15:D4 set the 12 bits of the threshold.
 *
 */
RegOVthrSet::RegOVthrSet(uint8_t nd) : Register(nd)
{
  init(0x19);
}

// shift the threshold value by 4 bits
boolean RegOVthrSet::writeAllDevices()
{
  uint16_t value = (ThrValue << 4);
  return Register::writeAllDevices(value);
}

void RegOVthrSet::setThreshold(uint16_t value)
{
  ThrValue = value;
}

void RegOVthrSet::setThreshold(float value)
{
  ThrValue = (value / ADC_VOLTAGE_RESOLUTION);
}

/*
 * ================== RegOVthrClear
 *
 * Address = 18
 * Bits D15:D4 set the 12 bits of the threshold.
 *
 */
RegOVthrClear::RegOVthrClear(uint8_t nd) : Register(nd)
{
  init(0x18);
}

// shift the threshold value by 4 bits
boolean RegOVthrClear::writeAllDevices()
{
  uint16_t value = (ThrValue << 4);
  return Register::writeAllDevices(value);
}

void RegOVthrClear::setThreshold(uint16_t value)
{
  ThrValue = value;
}

void RegOVthrClear::setThreshold(float value)
{
  ThrValue = (value / ADC_VOLTAGE_RESOLUTION);
}

/*
 * ================== RegAlrtUVen
 */

RegAlrtUVen::RegAlrtUVen(uint8_t nd)
  : Register(nd)
{
  init(0x07);
}

// Set all bits D11:D0 to enable all cells
boolean RegAlrtUVen::enableAllCells() {
  return Register::writeAllDevices(0x0FFF);
}

boolean RegAlrtUVen::enableCells(uint16_t cellNumberMask)
{
  return Register::writeAllDevices(cellNumberMask);
}

boolean RegAlrtUVen::enableCells(uint16_t cellNumberMask, 
                               uint8_t deviceNum)
{
  return writeDevice(cellNumberMask, deviceNum);
}

